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1.  Introduction 


This  report  extends  the  work  of  Ballistics  Filtering  ARL-TR-47351  by  showing  the  process  of 
realizing  the  ideas  through  a  simulation.  By  providing  a  concrete  example  it  is  hoped  other 
realizations  of  the  ideas  can  be  pursued  with  a  reasonable  effort.  Ballistic  Filtering  describes  the 
dynamic  equations  that  can  be  used  to  form  Extended  Kalman  Filters  (EKF)  for  the  estimation  of 
a  projectile’s  trajectory.  The  steps  associated  with  initialization  and  implementing  an  EKE  are 
demonstrated  through  a  specific  task.  The  performance  of  an  EKE  processing  Global  Positioning 
System  (GPS)  observation  is  compared  to  the  performance  of  an  EKE  processing  both  GPS  and 
axial  accelerometer  observations.  Hit  point  prediction  error  is  used  as  the  measure  of 
effectiveness.  Both  filters  use  the  same  dynamics  for  state  and  covariance  propagation. 

Commented  code  is  included  as  appendix  A  to  allow  the  reader  to  observe  the  details  of  the 
implementation.  It  is  assumed  that  the  reader  has  examined  the  Ballistics  Eiltering  report^  and 
can  reference  the  report.  The  best  way  to  think  of  this  implementation  is  as  a  process  that 
propagates  the  state  and  state  covariance  that  is  interrupted  from  time  to  time  by  the  task  of 
processing  observations.  The  implementation  of  an  extended  filter  is  conveyed  symbolically  by 
the  following  set  of  equations.  All  symbols  represent  vectors  or  matrices  so  the  operations  are 
those  of  linear  algebra  and  are  only  scalar  operations  if  the  quantities  represented  are  scalar.  It  is 
assumed  the  reader  is  familiar  with  linear  algebra;  Gilbert  Strang  has  authored  many  excellent 
books  on  linear  algebra.  To  develop  an  EKE  it  is  necessary  to  have  access  to  a  set  of  linear 
algebra  routines  as  an  EKE  is  described  with  linear  algebra  operations.  Einpack  and  Eispack  are 
two  well-known  linear  algebra  packages. 


2.  The  Extended  Filter  Overview 


2.1  Initial  Conditions 

The  symbol  x  is  used  to  denote  the  state  of  the  system  and  is  assumed  to  have  a  normal 
distribution;  the denotes  “is  distributed  as”  and  the  N(a,b)  denotes  a  normal  distribution  with  a 
mean  of  a  and  covariance  of  b.  The  hat  denotes,  the  estimate  of,  so  x  symbolizes  the  estimate  of 
the  state: 

x(0)~A(x(0),P(0)).  (1) 

The  above  statement  indicates  that  to  start  the  filter  both  the  state,  (a  vector),  and  the  state 
covariance  matrix,  need  to  be  input.  The  projectile  state  in  this  example  contains  three  location 

-1 

^  Thompson,  A.  Ballistics  Filtering',  ARL-TR-4735;  U.S.  Army  Research  Laboratory:  Aberdeen  Proving  Ground,  MD,  2009. 
2  Ibid. 
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parameters,  three  velocity  parameters,  and  one  parameter  representing  the  projectile  drag 
coefficient.  The  estimate  of  the  state  is  based  on  knowledge  of  the  initial  conditions;  for 
example,  the  estimate  of  the  launch  site’s  location,  the  estimate  of  the  launch  velocity,  and  the 
estimate  of  the  drag  factor.  The  state  covariance  can  be  estimated  based  on  knowledge  of  the 
techniques  used  to  estimate  the  initial  state  value.  The  position  variance  should  be  based  on  the 
location  method  used  to  determine  the  launch  site.  Velocity  information  would  be  based  on  the 
uncertainty  associated  with  the  gun  tube  direction,  tip  off  at  barrel  exit,  and  muzzle  velocity 
uncertainty,  etc.  The  uncertainty  associated  with  the  drag  factor  can  be  approximated  via 
knowledge  of  model  shortcomings  or  from  recursively  simulating  a  launch  and  then  empirically 
setting  the  variance.  As  time  progresses  the  importance  of  these  values  diminishes;  however,  it 
is  important  to  get  a  reasonable  start,  especially  when  using  an  EKF. 

2.2  Time  Propagation 

The  first  section  of  code  is  related  to  change  of  the  state  and  the  state  covariance  as  a  result  of  the 
dynamics  or  the  plant.  These  can  be  repeated  between  observations  to  minimize  the  nonlinear 
effects.  For  the  case  investigated  there  are  10  propagation  updates  per  GPS  observation  update. 

System  nonlinear  dynamics  plus  plant  noise  q~N(0,Q)  is  represented  by  the  following  vector 
relationship. 

x{t^)  =  f{x{t^_^),t)  +  q(t)  (2) 

h  (3) 

The  covariance  propagation  for  an  EKF  is  updated  through  the  following  matrix  relationship. 

P(t,)  =  F(x(t, ),  )  +  P(t,_,  )F(x(t, ),  0  +  Q(t, )  (4) 

P(t,)  =  P(t,_,)  +  P(t,)At  (5) 

Both  the  state  and  the  state  covariance  are  updated  in  the  manner  of  Euler  integration;  that  is,  the 
updated  values  are  equal  to  the  old  value  added  to  the  time  interval  multiplied  by  the  differential. 
If  an  observation  is  not  available  the  above  steps  are  repeated  until  a  sensor  presents  an 
observation  to  the  filter.  Also,  to  predict  the  future  values  of  the  state  the  above  equations  would 
be  propagated  forward  in  time  and  provide  an  estimate  of  the  state  and  the  state  covariance. 

From  a  pragmatic  perspective,  the  Q  matrix  is  used  to  account  for  the  shortcomings  of  the 
dynamic  model  used  for  the  state.  It  has  the  effect  of  preventing  the  state  covariance  from 
becoming  very  small.  If  the  state  covariance  becomes  too  small  then  it  will  effectively  ignore 
the  observations;  this  is  referred  to  as  the  closing  of  the  filter.  It  can  be  amusing  to  think  of  a 
filter  as  being  narrow  minded. 

The  next  line  defines  the  matrix  F(x(0,0  ,  this  matrix  needs  to  be  available  each  time  step  for 
propagation.  Computationally  this  is  the  most  expensive  step  in  the  filter  so  it  is  worthy  of  effort 
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to  find  ways  to  minimize  the  number  of  times  this  computation  is  made.  Ideas  for  this  can  be 
based  on  the  state  value  or  on  information  related  to  change  in  the  F{x{t),t)  matrix.  Ideas  like 

these  account  for  some  of  the  variation  in  EKF  implementations. 


F(x(t^ ),  0  -  1-^(0  =  )  (6) 

dx{t) 

Since  both  f{x{t),t)  and  x(0  are  vector  quantities  the  resulting  matrix  F{x{t^),t)  will  be  a 
square  matrix  of  the  same  dimension  as  the  state.  In  the  single  element  “ij”  notation  the  previous 
matrix  equation  can  be  expressed  as: 

Fij  (x(F ),  0  -  '  \x(t)  -  x(t, ) .  (7) 


2.3  The  Observation  Phase 

The  next  set  of  equations  represents  the  steps  taken  to  update  the  state  when  an  observation 
becomes  available  for  processing.  While  the  time  propagation  produces  continuous  change,  the 
observation  updates  result  in  discontinuities  in  both  the  state  and  the  state  covariance.  There  will 
be  a  jump  in  the  state  value  and  the  state  covariance  will  be  instantly  diminished  as  the 
consequence  of  processing  an  observation.  The  observation  and  state  are  combined  in  a  least 
squares  fashion  based  on  their  respective  covariances  (see  Thompson  1991  BRL-TR-33033).  It 
is  worth  noting  that  the  accuracy  of  the  estimate  depends  on  precise  values  of  the  state  and 
observation  covariance.  If  the  covariance  values  are  precise  then  the  result  is  an  optimal 
estimate.  The  processing  of  an  observation  is  considered  to  take  place  at  a  specific  time  step. 
There  is  a  need  to  distinguish  between  the  values  of  the  state  and  the  state  covariance  before  and 
after  an  observation  is  processed.  This  distinction  is  only  needed  for  two  formulas. 

Although  observations  can  be  scalar  quantities,  they  are  generally  considered  as  vectors,  and  z 
is  assumed  to  be  a  vector.  This  is  the  observation  equation  v~N(0,R) 

z(F) -/i(x(F),F)  +  v(F).  (8) 

In  equation  8  the  error  is  considered  a  sample  from  the  normal  distribution  with  zero  mean  and 
covariance  R.  Obviously  R  represents  the  measurement  error  and  it  is  typical  to  include  a  model 
or  subroutine  to  calculate  R  for  each  observation.  In  general  the  elements  of  the  R  matrix  are: 

Fu  (h )  =  Pij  (h  (h  )^j  (h )  >  (9) 

where  the  matrix  R  is  square,  a.  represents  the  standard  deviation  of  the  ith  observation,  and  the 
correlation  between  measurements  is  represented  by  p.j .  The  dimension  of  R  is  the  number  of 


^  Thompson,  III,  A.  A.  Data  Fusion  for  Least  Squares',  BRL-TR-3303;  U.S.  Army  Ballistic  Research  Laboratory:  Aberdeen 
Proving  Ground,  MD,  1991. 
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measurements  made  by  a  sensor  at  each  time  step.  For  GPS  location  measurements  R  can  be 
treated  as  a  constant  diagonal  matrix  of  dimension  3. 

Relinearize  the  observation: 


dh(x(t)) 

dx(t) 


(10) 


The  dimension  of  this  matrix  will  be  the  dimension  of  the  observation,  z  ,  by  the  dimension  of 
the  state,  x(t) .  The  gain  due  to  an  observation  is: 

K(t, )  =  P(t,  )H  m,  ))[H{x{t,  ))P{t,  )H  {x{t, ))  +  R{t, )]  ' ,  (11) 


This  matrix  formula  can  be  shown  to  be  a  least  squares  formulation  for  recursively  processing 
observations.  Note  that  exponent  -1  represents  matrix  inversion  and  the  operation  is  the  matrix 
transpose  operator. 

2.4  Change  in  the  State  Due  to  Observation 

The  preobservation  value  of  the  state  needs  to  be  distinguished  from  the  value  of  the  state  after 
the  observation  is  processed.  In  this  notation  the  minus  sign  (-)  indicates  previous  to  the 
observation  being  processed,  the  subscript  k  indicates  that  t  ,  and  the  plus  sign  (+)  indicates 

after  the  observation  has  been  processed: 

x{r^)^x{t^)  (12) 

x{tl)^x{r^)  +  K{t^){z{ti^)-h{x{r^\t^))  (13) 

x(tj  =  x,(t;).  (14) 


Update  the  state  covariance  via  observation,  the  symbol  I  is  used  to  represent  the  identity  matrix 
for  this  example  it  would  be  seven  dimensional  or  .  The  portion  of  the  formula, 

)  -  h{x{t^  )T«;  ) ,  is  sometimes  referred  to  as  the  innovation  as  it  represents  the  new 
information  being  incorporated  into  the  state.  For  the  state  covariance  the  following  matrix 
operations  take  place: 


pit:)=p{t,) 

(15) 

P{tl )  =  [/  -  K{t,  )HCx{t, ),  t,  )]P(t, ) 

(16) 

P(h)-P(0. 

(17) 

The  observation  phase  of  the  code  can  be  repeated  for  different  sensors  or  measurements;  the 
observation  matrix  and  the  measurement  covariance  would  differ  for  different  sensors.  The 
observation  cycle  of  the  individual  sensors  must  be  considered  in  the  design  of  an  EKF.  It  is 
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easy  to  envision  an  EKF  moving  along  with  state  propagation  but  using  an  interrupt  system  to 
process  specific  observations.  The  previous  perception  makes  the  design  of  an  EKF  for  variable 
time  steps  straightforward.  Figure  1  presents  a  summary  of  an  EKF  in  flowchart  form. 


Figure  1 .  Summary  chart  for  a  EKF. 
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3.  Scenario 


The  first  scenario  is  to  investigate  the  performance  of  an  EKF  processing  GPS  measurements  to 
predict  the  hit  point  of  a  mortar  trajectory.  The  second  scenario  adds  an  axial  accelerometer  to 
the  sensor  suite.  The  actual  trajectory  was  generated  from  a  6dof  model.  The  points  on  this 
trajectory  will  be  used  to  generate  observations  for  the  EKF.  By  using  a  6dof  model,  there  will 
be  nonlinearities  in  the  trajectory  that  are  not  modeled  by  the  EKF.  In  this  scenario  a  simple 
state  model  will  be  used.  Most  of  the  variation  in  EKF  implementation  is  due  to  the  dynamics 
used.  The  selection  of  dynamics  is  a  tradeoff  between  model  fidelity,  functionality,  and  speed  of 
computation. 

For  this  scenario  the  state  will  consist  of  seven  dimensions  location,  velocity,  and  ballistic 
coefficient.  The  down  range  direction  is  given  by  x^ .  The  vertical  direction  is  given  by  Xj  •  The 
cross  range  direction  is  given  by  .  The  only  effect  captured  will  be  the  drag;  this  is  done 

through  the  ballistic  factor  using  universal  drag  curves.  Other  drag  models  could  be  used,  but 
this  presents  a  default  model  that  works  well  with  artillery  and  mortar  rounds.  The  uncertainty 
due  to  unmodeled  dynamics  is  captured  by  the  matrix  q(t).  The  following  equations  are  used  to 
model  the  dynamics,  sometimes  this  is  referred  to  as  the  plant.  The  dynamic  models  used  are 
taken  directly  from  STANAG  4355^  or  are  simplified  versions  of  the  dynamics  presented 
therein.  A  detailed  description  of  the  units  for  a  more  complete  set  of  models  can  also  be  found 
in  STANAG  4355. 


/l 

(18) 

II 

(19) 

>< 

II 

(20) 

h  ^  -x^Ak.Vx^  -  ^  -  2Q^.X6 

(21) 

-x,Ak,Vx,  -g(l-  ^ 

(22) 

-  -x^Ak^Vx^  20^X5  +  20^X4 

(23) 

0 

II 

(24) 

NATO  STANAG  4355,  Modified  Point  Mass  Trajectory  Model,  North  Atlantic  Treaty  Organization,  January  20,  1997. 
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These  equations,  represented  above  as  f{x{t),t) ,  incorporate  drag,  gravity,  and  Coriolis  Effect; 
other  elements  of  the  dynamics  such  as  lift  and  Magnus  effect  are  ignored. 

This  scenario  will  model  a  mortar  round  fired  north  from  a  latitude  of  45  degrees  north.  The  shot 
elevation  is  45  degrees  with  a  speed  of  220  meters  per  second  (m/s).  The  launch  conditions  are 
used  to  initialize  projectile  state  vector x(to ) .  The  state  covariance  matrix  Pit^)  can  be 

estimated  based  on  knowledge  of  the  techniques  used  to  estimate  the  initial  state  value.  The 
position  variance  should  be  based  on  the  location  method  used  to  determine  the  launch  site. 
Velocity  information  would  be  based  on  the  uncertainty  associated  with  the  gun  tube  direction, 
tip  off  at  barrel  exit,  and  muzzle  velocity  uncertainty,  etc.  The  uncertainty  associated  with  the 
drag  factor  can  be  approximated  via  knowledge  of  model  shortcomings  or  from  recursively 
simulating  a  launch  and  then  empirically  setting  the  variance.  As  time  progresses  the  importance 
of  these  values  diminishes;  however,  it  is  important  to  get  a  reasonable  start,  especially  when 
using  an  EKE. 

Although  a  GPS  sensor  typically  makes  one  observation  a  second,  they  can  easily  be 
programmed  to  present  5  observations  a  second  and  with  some  effort  they  can  make  10  or  more. 
It  is  assumed  that  the  round  contains  a  GPS  sensor  that  can  make  10  independent  observations 
per  second.  The  ability  of  the  filter  to  predict  impact  point  will  be  observed.  If  the  filter 
performance  is  not  adequate  then  it  can  be  concluded  that  a  GPS  receiver  is  not  adequate  given 
the  chosen  dynamics. 

In  order  to  propagate  the  state  covariance  the  partials  of  the  dynamic  equations  with  respect  to 
the  state  are  needed  the  needed  partials  are: 


II 

(25) 

II 

(26) 

II 

(27) 

F  = 

K  ’ 

(28) 

p  -  -X  k  Vx 

0X2 

(29) 

^  dk  ^ 

F44  -  XjA  Vx,^+  k^x^  +KV  , 

y  SX4  ^X^  ^ 

(30) 

f  8k 

F,,  =  -x,Ax,  V^  +  k,—  , 

1  0x5  dx,  j 

(31) 

7 


^46  ^  -^7^4 


f 


dk,  ,  dv'" 


y—+K— 

5xg  dx, 


-2Q^ 


-6/ 


F47  =  -AkjVx^ , 


F  -  — 

^51 


X, 


2/?,  (x,^  +  X3Y  ’ 

^52  ~  ~^l^dy^5 


DA 

5X2 


F  =  — 
^53 


F54  -  -X2^5 


2F,(x>X3Y’ 

5x4 


dx. 


dx, 


■4  J 


^55  ^1^ 
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The  following  information  is  also  needed  to  obtain  numerical  values  for  the  F-matrix 


(32) 

(33) 

(34) 

(35) 

(36) 

(37) 

(38) 

(39) 

(40) 

(41) 

(42) 

(43) 

(44) 

(45) 

(46) 
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{4,5,6}’ 


(47) 


dV  _  X; 

5x,.  {xl  +  xl+xl)'^ 

Qq  =  1.223 
a^  =  1.071e-4 
A  = 

5A 

—  =  -«o«i«’  ' 

5X2 

g  -  9.80665(1  -  .0026cos(2LaO) , 
=6356766. 


(48) 


(49) 

(50) 


A  is  the  air  density  as  a  function  of  altitude  X2 ,  ^  is  the  acceleration  of  gravity  as  a  function  of 
altitude,  and  is  the  radius  of  the  Earth.  The  drag  coefficient  is  calculated  using  a  fourth 
degree  polynomial  of  Mach  number  m  .  Mach  number  is  the  speed  divided  by  the  speed  of 
sound.  Notice  that  in  this  formulation  the  partial  of  the  speed  of  sound  with  respect  to  height  is 
not  included  in  the  F-matrix. 


Sq  =  340.3  temperture  =  59°  F 
x^i  =  hieght(launchabovesealevel) 
c^=2.26e-5 
s  =  5o(l-c,(x,,  +  X2))'^ 


c,.m' 


r  4 


-1 


V  i=i  y 


(51) 


Q  =  7.292k -5 

=  Qcos(to)  (52) 

Fly  =  Qsin(to) 

i'o  is  the  speed  of  sound  a  sea  level  in  m/s  and  s  is  the  speed  of  sound  at  altitude  x^^+Xj  ■  is 
the  drag  coefficient  is  calculate  using  a  4*  order  polynomial  fit.  Qj  and  Qj  the  Coriolis 
factor  in  the  1=1  and  1=2  directions. 
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In  the  example,  the  Q  matrix  is  assumed  to  be  constant  in  time.  There  is  assumed  to  be  no 
correlation  between  errors  so  all  of  the  off  diagonal  terms  are  zero.  All  of  the  main  diagonal 
terms  are  set  to  0.05: 


0.05 

0 

0 

0 

0 

0 

0 


0 

0.05 

0 

0 

0 

0 

0 


0 

0 

0.05 

0 

0 

0 

0 


0 

0 

0 

0.05 

0 

0 

0 


0 

0 

0 

0 

0.05 

0 

0 


0 

0 

0 

0 

0 

0.05 

0 


0 

0 

0 

0 

0 

0 

0.05 


(53) 


In  this  example  the  measurements  to  be  used  are  assumed  to  be  estimates  of  position  from  a  GPS 
receiver.  It  has  been  assumed  that  observations  are  independent  of  one  another  and  constant  in 
time.  Neither  of  these  assumptions  is  true  in  a  real  GPS  but  have  been  made  here  to  simplify  the 
calculation.  The  R  matrix  is  constant  over  time: 


4  0  0 
0  9  0 
0  0  4 


(54) 


The  observation  matrix  H  is  expressed  as  follows: 


dz, 

dz, 

dz, 

5z, 

^x^ 

dx2 

dx^ 

5X5 

5X6 

dxj 

fl 

0 

0 

0 

0 

0 

0] 

dz2 

SZ2 

dZ2 

^Z2 

dz2 

_ 

0 

1 

0 

0 

0 

0 

0 

(55) 

dxj 

dx. 

dx^ 

dx. 

dx. 

dx. 

dxj 

dz^ 

dz^ 

dz2 

dz^ 

5z3 

5z3 

[o 

0 

1 

0 

0 

0 

oj 

dxj 

dx^ 

dx^ 

dx, 

5X6 

-j 

_ 

In  this  case  the  simplicity  is  the  result  of  the  measurements  being  the  same  as  the  position 
portion  of  the  state  variable.  Throughout  the  remainder  of  this  report  a  task  will  be  mentioned 
and  then  the  code  associated  with  that  task  will  be  discussed. 


4.  Initialization 


The  initialization  problem  can  be  subdivided  into  two  major  sections:  those  tasks  associated 
with  the  dynamics  being  used,  and  those  associated  with  the  operation  of  the  filter.  The  routine 
INIT8  sets  up  the  universal  or  default  model  for  drag.  In  other  implementations  models  for  lift 
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and  spin  may  be  required  and  this  would  be  a  logical  spot  to  include  them.  The  drag  model  is  a 
polynomial  fit  between  breakpoints;  thus,  the  shape  of  the  curve  is  generated  through  these 
models.  The  data  gives  the  breakpoints  and  the  coefficients  of  the  fitted  polynomials  between 
each  set  of  breakpoints 

The  task  of  the  routine  INIT  is  to  set  up  variables  for  the  filter;  most  of  these  are  related  to  the 
particular  scenario.  Line  7  perturbs  the  assumed  starting  point  from  the  actual  starting  point. 
Lines  8-12  set  up  the  assumed  original  velocity.  The  actual  6dof  was  fired  at  45  degrees  so  there 
is  4  degrees  of  error  included.  Line  13  initializes  the  value  of  the  ballistic  factor.  Line  15  sets  up 
the  default  speed  of  sound.  Lines  18-21  set  up  the  q  matrix.  The  q  matrix  represents  the  short 
comings  of  the  chosen  dynamics;  part  of  designing  an  EKF  is  the  process  of  adjusting  the  q 
matrix  to  a  satisfactory  value.  There  is  rarely  a  way  to  predetermine  the  q  matrix;  typically 
simulations  or  multiple  runs  are  used  to  tune  these  values.  Lines  25-27  are  the  model  for  the 
observation  error.  These  models  can  be  quite  complex  and  may  need  to  consider  the  situational 
geometry.  For  this  scenario  GPS  observations  are  assumed  to  be  for  position  and  have  the  same 
error  structure  from  observation  to  observation;  thus,  the  h  matrix  will  not  change  from 
observation  to  observation.  Fines  29-30  define  the  h  matrix;  this  is  the  observation  in  terms  of 
the  state  variables.  Since  GPS  gives  position  this  matrix  just  selects  the  three  position  variables 
and  ignores  the  other  state  variables.  Fine  32  is  just  to  define  an  identity  matrix  of  the  same 
order  as  the  state.  The  last  task  of  the  INIT  routine  is  to  define  the  state  uncertainty.  The  p 
matrix  is  used  to  represent  state  covariance.  The  use  of  ancillary  knowledge  and/or  simulations 
can  indicate  good  values  for  the  initial  state  covariance.  In  this  case  each  position  error  and  each 
velocity  error  is  assumed  to  have  a  variance  of  1.  The  variance  of  the  ballistic  coefficient  is 
assumed  to  be  .0001.  This  value  was  chosen  by  observing  the  variation  in  this  parameter  over 
several  simulations  and  selecting  a  value  that  reflected  the  observed  variation. 

4.1  Main  Program:  driver? 

After  initialization  the  main  program  starts.  This  is  basically  a  loop  that  processes  the 
observations.  Typically  in  an  FKF  each  observation  is  associated  with  a  cycle  of  the  filter; 
however,  in  this  example  there  are  10  sub  cycles  associated  with  each  observation.  These  sub 
cycles  can  be  thought  of  as  a  way  to  minimize  the  effects  of  linearization.  Rather  than  one  large 
step  being  taken  between  observations  the  interval  is  subdivided  to  minimize  the  error  associated 
with  the  assumption  of  linearity.  The  basic  time  step  is  1/100  of  a  second  and  the  observations 
come  at  the  rate  of  10  per  second.  Fine  4  calls  the  previously  discussed  initialization  routines. 
Fine  5  sets  the  time  step.  Fine  9  loads  the  observations.  At  this  point  in  the  program  this  is  the 
true  trajectory  that  was  generated  from  a  6dof  model.  Noise  will  be  added  to  these  values  before 
they  are  fed  to  the  FKF  as  observations.  Fines  1 1-12  set  the  earth’s  latitude  and  radius.  In  lines 
13-14  variables  related  to  wind  speed  are  set  to  zero.  Fine  16  sets  constants  that  are  used  for  the 
standard  atmosphere  model  so  air  density  can  be  calculated.  Fines  18-30  set  variables  used  for 
Mach  number,  the  gravity  model,  and  Coriolis.  Fine  31  dimensions  the  f  matrix  and  initializes  it 
to  be  all  zeros.  Fines  33-35  initialize  a  set  of  record  keeping  variables.  These  are  for  the  state. 
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the  predictions,  and  the  trace  of  the  covariance  matrix.  Line  40  initializes  a  counter  for  between 
observation  updates  of  the  state  and  the  state  covariance.  There  are  10  propagation  updates  for 
each  observation  update.  Line  45  initializes  a  counter  for  observations. 

The  main  loop  is  controlled  by  the  height  of  the  projectile,  once  the  height  returns  to  ground 
level  the  simulation  ends.  Lines  50-54  get  the  air  pressure,  velocity,  and  Mach  number.  Lines 
55-56  the  drag  is  calculated.  Line  57  calls  a  routine  to  calculate  the  f  matrix.  This  is  needed  for 
the  covariance  propagation.  Line  58  calls  a  routine  to  get  the  change  in  the  state.  The  state  and 
state  covariance  are  propagated  in  lines  59  and  60.  Line  61  increments  the  observation 
subinterval  counter. 

The  steps  to  account  for  an  observation  are  in  lines  63-79.  Lines  66  and  67  create  the 
observation  by  adding  noise  to  the  actual  location.  After  this  the  observation  subinterval  counter 
is  reset  in  line  69.  The  gain  of  information  due  to  the  observation  is  represented  by  the  matrix  K; 
this  is  calculated  in  line  72.  The  new  state  based  on  a  least  squares  combination  of  the  state  and 
the  observation  is  calculated  on  line  73.  The  new  state  covariance  that  includes  the  reduction 
due  to  processing  the  observation  is  calculated  on  line74.  Line  77  calls  the  prediction  routine. 
The  remaining  lines  update  variables  associated  with  record  keeping  and  closing  loops. 

4.2  The  Function:  getudrag 

This  function  is  used  to  find  the  drag  based  on  a  universal  drag  curve.  The  basic  information 
was  read  in  the  routine  initS.  This  routine  is  basically  a  binary  search  to  select  the  correct  set  of 
drag  coefficients.  Lines  7-31  select  the  proper  row  of  the  drag  matrix.  Once  this  is  completed 
the  interpolation  polynomials  are  set  up  for  the  drag  and  the  derivative  of  the  drag  and  the  values 
are  calculated.  Lines  33-35  complete  the  calculation. 

4.3  The  Function:  S7calcF 

The  rational  for  the  calculations  in  this  routine  are  discussed  in  the  report  Ballistic  Filtering. 
Basically  the  rows  are  the  partials  of  the  dynamic  equations  with  respect  to  the  state  variables. 
Partials  that  are  small  across  the  entire  trajectory  can  be  dropped  with  small  effect;  of  course 
doing  this  depends  on  the  application.  The  savings  in  computation  time  can  be  considerable. 

First  partials  of  the  velocity  are  found  and  then  partials  of  the  Mach  number.  These  are  done  in 
lines  8  to  16.  Line  18  states  in  terms  of  the  state  that  the  change  in  position  is  due  to  the  velocity. 
The  partials  of  equation  46  from  Ballistic  Filtering^  are  calculated  in  lines  22-27.  The  rest  of  the 
function  performs  similar  calculations  for  the  other  components  of  velocity. 

4.4  The  Function:  S7dx 

This  function  calculates  the  change  in  the  state  based  upon  the  dynamics. 


^  Thompson,  A.  Ballistics  Filtering',  ARL-TR-4735;  U.S.  Army  Research  Laboratory:  Aberdeen  Proving  Ground,  MD,  2009. 
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4.5  Simulation  Results:  GPS  Only 


The  simulation  described  above  was  used  to  investigate  the  performance  of  the  seven  state 
ballistic  EKF.  Most  GPS  sensors  give  yield  one  sample  per  second;  it  is  a  minor  modification  to 
get  five  observations  per  second,  and  possible  to  obtain  10  per  second.  A  sample  rate  of  10  was 
chosen  as  this  will  be  the  best  possible  situation.  The  results  are  summarized  in  figure  2.  After 
an  initial  increase  the  error  decreases  exponentially. 


Figure  2.  Error  in  prediction  of  hit  point. 


The  range  error  grows  for  about  5  seconds  (s)  peaking  at  about  300  meters  (m)  before  it  starts  to 
diminish  to  an  error  of -.35  m  at  the  end  of  flight.  After  22  s  the  error  is  approximately 
20  m  and  after  28  s  the  error  is  less  than  3  m.  The  performance  of  an  EKF  typically  provides 
estimates  that  other  subsystems  utilize;  and  thus  directly  influences  system  performance. 


5.  Accelerometer  Observations 


Next  the  effects  of  adding  an  accelerometer  to  the  sensing  unit  are  investigated.  Appendix  B 
includes  the  additional  and  altered  routines  for  this  situation.  An  accelerometer  aligned  with  the 
spin  axis  is  assumed  to  be  aligned  with  the  velocity.  This  will  never  be  true  as  precession  and 
nutation  result  in  the  spin  axis  oscillating  around  the  direction  of  motion.  The  angle  between  the 
spin  axis  and  the  velocity  vector  is  typically  small — on  the  order  of  a  few  degrees.  This  angle  is 
also  related  to  the  spin  of  the  projectile  with  lower  spin  being  associated  with  larger  angles.  In 
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this  simulation  the  attainable  improvement  in  accuracy  due  to  including  an  axial  accelerometer 
on  a  projectile  that  already  contains  GPS  is  investigated. 


The  force  seen  by  the  accelerometer  is  the  inner  product  of  the  force  vector  and  the  normalized 
orientation  of  the  sensing  direction  of  the  accelerometer.  Assuming  the  accelerometer  is  on  the 
spin  axis  and  the  spin  axis  is  pointing  in  the  direction  of  velocity  will  present  the  best  possible 
case.  In  this  situation  the  force  seen  by  the  accelerometer  is  the  inner  product  of  the  force  and 
the  normalized  velocity.  The  previous  equations  /g  give  the  force  vector  in  terms  of  state 


variables,  and  the  direction  of  the  velocity  can  be  represented  as  the  vector 


where  v  is  the  magnitude  of  the  velocity.  The  inner  product  of  these  two  quantities  results  in  the 
following  lengthy  expression. 


, ,  x,  ex,  X4  X4  , ,  X, 

-  x^Ak^vx^ - 2Qy - XjAk^vx^ - g 

R 


V 


, ,  X.  ex,  X. 
-x,Ak,v^-^^ 
V  Rv 


’'-^5 

V  '  V  V 


1- 


(xf  +  X3  )'  ) 

2R. 


- h  20  ^XfT  — 


-2Q_^X5^  +  2Q  X4^ 

V  V 


(56) 


Taking  the  partial  of  the  above  expression  with  respect  to  each  of  the  state  variables  gives  the 
elements  of  the  observation  matrix  for  axial  accelerometer  measurements.  This  will  be  a  row 
rather  than  a  column.  Note  that  the  values  of  h  have  been  simplified. 
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An  examination  of  the  values  of  H  indieates  that  will  have  the  largest  magnitude;  thus  axial 

aeeelerometer  measurements  will  have  a  relatively  large  influenee  on  the  estimate  of  the  ballistie 
factor,  or  terms  directly  related  to  drag.  Since  drag  and  an  axial  accelerometer  both  work  along 
the  direction  of  the  velocity  vector,  this  is  not  too  surprising. 

The  modification  to  the  previous  ECF  to  accommodate  the  new  axial  accelerometer  observations 
is  straightforward.  A  new  block  is  added  to  process  the  observations  at  the  proper  time  intervals. 
In  this  situation  the  previously  used  blocks  for  state  propagation  and  GPS  observation  processing 
remain  unchanged.  The  GPS  observations  are  processed  before  the  accelerometer  readings  when 
both  observations  are  available  at  the  same  instance  of  time.  An  examination  of  the  routine 
driver? gpsacc  demonstrates  how  to  extend  the  previous  EKF  to  process  axial  accelerometer 
observations.  The  routine  accel  is  used  to  generate  the  force  seen  by  the  accelerometer.  The 
routine  hmatrix  generates  the  H-matrix  for  each  accelerometer  observation. 

A  comparison  of  figure  2  and  figure  3  illustrates  the  change  in  prediction  error  due  to  including 
an  axial  accelerometer.  The  new  sensor  suite  has  a  lower  maximum. 


Figure  3.  Hit  point  error  for  an  EKF  processing  GPS  and  an  accelerometer  error. 


The  hit  point  prediction  error  is  similar  to  the  GPS  only  EKF  after  15  s.  The  advantages 
associated  with  adding  an  accelerometer  can  only  be  assessed  with  respect  to  a  specific  system. 

Figure  4  shows  the  estimate  of  ballistic  factor  as  a  function  of  time.  This  figure  is  of  interest 
because  it  shows  the  filter  adjusting  the  overall  drag  based  on  the  observations.  Given  a  perfect 
model  of  the  drag  this  value  would  be  constant.  The  filter  diminishes  the  ballistic  factor  for 
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5  s  and  then  increases  it  for  the  remainder  of  the  flight.  This  gives  an  indication  of  some  of  the 
shortcomings  of  the  seven  dimensional  point  mass  dynamics  attempting  to  mimic  the  dynamics 
of  a  6dof  model. 

Appendix  C  includes  additional  routines  that  may  be  of  interest.  Included  are  simplified  models 
for  drift  and  spin.  These  routines  provide  a  default  capability. 


Figure  4.  Estimate  of  ballistic  factor. 


6.  Conclusions 


There  is  no  precise  step-by-step  way  to  design  a  Kalman  filter  (KF)  or  an  EKF.  This  paper  has 
demonstrated  one  path  through  the  set  of  possible  decisions  to  design  an  EKF.  It  is  evident  that 
there  are  many  possible  settings  that  can  and  should  be  investigated  in  setting  up  a  filter.  A 
central  question  is  the  quality  of  the  dynamics  used.  Usually  this  means  the  simplest  set  of 
dynamics  that  will  allow  the  system  to  get  the  job  done.  The  sensor  suite  has  a  huge  effect  on 
filter  performance  and  it  is  always  desirable  to  have  high  quality  measurements.  Also,  the 
timeliness  of  the  measurements  influences  filter  performance  in  that  many  observations  can 
offset  the  shortcomings  of  a  given  dynamics  model.  The  possibilities  to  investigate  seem  endless 
even  for  well  defined  problems;  thus  the  design  of  a  KF  or  an  EKF  is  typically  based  on  meeting 
system  requirements.  Conceptually  it  is  beneficial  to  conceive  of  the  filter  as  a  set  of  dynamic 
equations  that  get  interrupted  to  receive  corrections  based  on  observations. 
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In  this  investigation  an  EKF  processing  GPS  observations  was  compared  to  an  EKF  processing 
GPS  and  axial  accelerometer  observations.  The  results  showed  little  difference  for  the 
predictions  at  times  exceeding  15  s.  After  an  EKF  has  achieved  accurate  values  of  the  state 
additional  observations  will  not  add  much  value;  however,  additional  observations  will  keep  the 
state  from  drifting  away  from  the  true  values.  The  value  of  additional  information  depends  on 
how  it  related  to  the  state  through  the  observation  matrix,  the  covariance  of  the  observation,  and 
the  accuracy  of  the  state. 
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Intentionally  Lelt  Blank. 
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Appendix  A.  Code  for  an  EKF  processing  GPS 


Initx 

1  %function  y=initx  () 

2  %this  next  routine  sets  up  the  drag,  lift,  and  spin  curves 

3  initsS 

4  %the  state  will  contain  location  velocity  and  drag 

5  %set  up  state  variable 

6  x=zeros  (7,1); 

7  x(l:3)  =  [.01; .01; .01]  ; 

8  speed=220; 

9  el=49/180*pi; 

10  X ( 4 ) =speed*cos  (el ) ; 

11  X (5) =speed*sin  (el) ; 

12  x(6)=0; 

13  x(7)=l; 

14  %speed  of  sound  for  sea  level  about  53  degrees 

15  v_s=340.3; 

16  %terms  for  model  mismatch 

17  q=zeros  (7)  ; 

18  q(l,  l)=l;q(2,2)=l;q(3,3)=l; 

19  q(4,4)=l;q(5,5)=l;q(6,  6)=1; 

20  q=q*.05; 

21  %error  associated  with  the  observations 

22  %this  is  used  to  simulate  GPS  cband 

23  %r  is  the  observation  variance 

24  r=zeros (3) ; 

25  r (1, l)=4;r (2,2)=9;r (3,3)=4; 

26  r_sd=[2;3;2] ; 

27  %make  observation  matrix  in  terms  of  state 

2 8  h=zeros  (3,7)  ; 

29  h(l,  l)=l;h(2,2)=l;h(3,3)=l; 

30  %the  dimension  of  the  state  for  I  used  in  covariance  propagation 

31  I=eye (7) ; 

32  %set  up  initial  state  uncertainty 

33  p=eye ( 7 ) ; 

34  p=p*.001; 

35  p(l,l)=l;p(2,2)=l;p(3,3)=l; 

36  p(4,4)=l;p(5,5)=l;p(6,  6)=1; 

37  p(7,7)=.0001; 
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InitsS 

1  %drag  coefs  for  polynomial  to  estimate  drag 

2  drl=[ .27754e-4  0000]; 

3  dr2=[ .29407446e-3  - . 16856609e-2  .39541129e-2  - . 40706187e-2  . 154 97302e-2 ] ; 

4  dr3=[-. 56492074  .24140455el  -.38636028el  .27445913el  -.73004070]; 

5  dr4=[.16449122el  -.6472231el  .95427249el  -.62486232el  . 15332897el] ; 

6  dr5=[-.38991679e-2  .12251269e-l  - . 14008227e-l  .70697832e-2  - . 13323438e-2 ] ; 
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7  dr6=[.17693159e-3  - . 14042065e-3  .74643008e-4  - . 21621397e-4  . 26788426e-5] ; 

8  %drag  breakpoints  for  sets  of  coef 

9  udrag_bp=[.6  .9  .99  1.06  1.5  5] ; 

10  udrag= [drl;dr2;dr3;dr4;dr5;dr6] ; 
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DriverV 

1  %function  y=driver7() 

2  %do  the  first  intialization  phase 

3  initx 

4  dt=.01; 

5  %these  are  from  the  6dof  trajectory 

6  %obviously  in  real  time  they  would  not  be  known  in  advance 

7  %these  are  used  as  the  observation  feed 

8  load  obsdat 

9  lat=45/180*pi;  %adefault  value 

10  r_e=6378000;  h0=0; 

11  X  w=0;%0  indicates  no  wind 

12  y  w=0; 

13  %air  pressure 

14  ap  c0=1.223;  ap  c2=l . 07 le-4 ; 

15  %mach  number 

16  v_s0=340.3;  %59F 

17  V  c=2 . 26e-5; 

18  %gravity 

19  g0=9. 80665; 

20  gl=.0026; 

21  g=g0* (l-gl*cos (2*lat) ) ; 

22  %corriolis 

23  omega=7 . 2 92 le-5 ; 

24  om_v= [omega*cos (lat) ; omega*sin (lat) ; 0] ; 

25  %allocate  f-matrix 

2 6  f=zeros ( 7 ) ; 

27  state= [x] ; 

28  pre= [ ] ; 

29  ptrace=trace (p) ; 

30  %this  is  the  counter  for  between  observation  updates 

31  %this  helps  minimize  the  effects  of  nonlinearity 

32  %many  times  these  substeps  can  be  ignored  and  the  propagation  can 

33  %be  calculated  in  one  step  between  observations 

34  obs=0; 

35  %the  next  variable  just  counts  the  observations 

36  %the  observations  start  with  count+1 

37  %note  that  this  variable  can  be  used  to  start  the  observation  stream 

38  %at  any  desired  point  of  the  trajectory 

39  count=l; 

40  %main  loop 

41  while  X (2) >0 

42  %air  preasure  etc  is  triggered  by  altitude 

43  %anyhow  update  variables  that  change  as  the  state  changes 

44  air  p=ap  c0*exp(-ap  c2*x(2));  %current  air  pressure 

45  %velocity  and  mach  number 
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V  s=v  s0*(l-v  c* (x (2) +h0) ) ^ . 5; 

v=sqrt  (  (x  (4)  -x_w)  ''2+x  (5)  ^'2+  (x  (6)  -y_w)  ''2)  ; 

m=v/v  s;  %mach  number 

k;=getudrag  (m,  udrag,  udrag  bp);  %udrag&udrag_bp  need  2b  initialized 
kd=k(l) ;kd_m=k(2)  ; 

f=S7calcF (x,  air  p, kd, kd  m,v,m,r  e,om  v, ap  c2,g,f); 

dx=S7dx (x,  air  p,kd,v,r  e,om  v,g); 

x=x+dx*dt; 

p=propP (dt, f , p, q) ; 

obs=obs+l ; 

%this  inside  loop  is  triggered  by  a  GPS  observation  being  available 
if  obs==10 

count=count+l ; 

%add  some  error  to  the  observation 
z_er=diag (randn (3,1)) *r_sd* . 1; 
z=obsdat ( : , count) +z_er; 

%reset  counter 
obs=0 ; 

%if  r  is  variable  the  routine  getR  should  be  designed  to  model 

%the  observation  error  and  included  here 

K=getK (p, h, r ) ; 

x=x+K* (z-h*x) ; 

p= ( I-K*h) *p; 

%predict  routine  this  will  change  somewhat  based  on  specific 
%application 

%pos=predictX (x, r_e, om_v, hO, x_w, y_w, udrag, udrag_bp, g, dt, 5) ; 
pos=predictH (x, r_e, om_v, hO, x_w, y_w, udrag, udrag_bp, g, dt) ; 
pre=[pre  pos] ; 

end 

state=[state  x] ; 
ptrace= [ptrace  trace (p) ] ; 

end 

y=state([l  3  2],:); 
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Getudrag 

1  function  y=getudrag (m, drag, bp) 

2  %this  finds  the  drag  based  on  a  universal  drag  curve 

3  %the  operations  are  set  up  under  the  assumptin  that  most  of  the  time  is 

4  %spent  near  m=l  can  be  further  improved  by  inserting  the  actual  values 

5  %rather  than  passing  the  breakpoints 

6  row=l; 

7  if  m<bp ( 3 ) 


8 

if  m>bp (2 ) 

9 

row=3 ; 

10 

else 

11 

if  m>bp ( 1 ) 

12 

row=2 ; 

13 

else 

14 

row=l ; 

15 

end 

16 

17  else 

end 
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22 

23 

24 

25 

26 

27 

28 

29 

30 

31 

32 
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34 

35 


if  m<bp ( 4  ) 
row=4 ; 

else 

if  m<bp ( 5 ) 
row=5 ; 

else 

if  m<bp ( 6) 
row=6; 

else 

row=0 ; 

end 

end 

end 

end 

x= [ 1 ;  m;  m*m;  m*m*m;  m*m*m*m] ; 

dx= [ 1 ; 2  *m; 3*m*m; 4  *m*m*m] ; 

y= [drag (row, : ) *x  drag (row, 2:5) *dx] ; 
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S7calcF 

1  function  f=S7calcF (x,  air  p, kd, kd  m,v,m,r  e,om  v, ap  c2,g,f) 

2  ap  x2=-ap  c2*air  p; 

3  %v=sqrt  (xT4)  ^2+xT5)  ''2+x  (6)  "2)  ; 

4  %m=v/v_s; 

5  %partials  of  velocity  follow 

6  V  x4=x (4) /v; 

7  V  x5=x(5)/v;  %remember  x5  is  velocity  in  height  or  altitude 

8  v_x6=x ( 6) /v; 

9  %m_x2=-v/v_s''2*vs  x2; 

10  mv  x4=m*v  x4; 

11  mv  x5=m*v  x5; 

12  mv  x6=m*v  x6; 

13  f (1, 4)=l;f (2,5)=l;f (3, 6)=1; 

14  %start  calculation  of  the  F  matrix  Iguess  f  will  b  14d 

15  %only  the  4-6  rows  change  each  step 

16  %row  4 

17  f (4, l)=-g/r_e; 

18  f (4, 2) =-x (7) *v*x (4) *ap_x2*kd; 

19  f ( 4 , 4 ) =-x ( 7 ) *air  p* (v*kd  m*mv  x4*x(4)+v  x4*kd*x (4) +kd*v) ; 

20  f ( 4 , 5 ) =-x ( 7 ) *air  p*x(4)*(v*kd  m*mv  x5+v  x5*kd) ; 

21  f (4, 6) =-x (7) *air  p*x(4)*(v*kd  m*mv  x6+v  x6*kd)-2*om  v(2); 

22  f (4, 7) =-air_p*kd*v*x (4) ; 

23  %row  5 

24  f (5, 1) =g/ (2*r_e) *x (1) /sqrt (x ( 1 ) *x ( 1 ) +x ( 3 ) *x ( 3 ) ) ; 

25  f (5, 2) =-x (7) *v*x (5) *ap_x2*kd; 

26  f (5, 3) =g/ (2*r_e) *x (3) /sqrt (x ( 1 ) *x ( 1 ) +x ( 3 ) *x ( 3 ) ) ; 

27  f ( 5 , 4 ) =-x ( 7 ) *air  p*x(5)*(v*kd  m*mv  x4+v  x4*kd); 

28  f ( 5 , 5 ) =-x ( 7 ) *air  p* (v*kd  m*mv  x5*x(5)+v  x5*kd*x (5) +kd*v) ; 

29  f (5, 6) =-x (7) *air  p*x(5)*(v*kd  m*mv  x6+v  x6*kd)+2*om  v ( 1 ) ; 

30  f (5, 7) =-air_p*kd*v*x (5) ; 

31  %row  6 

32  f (6, 2) =-x (7) *v*x (6) *ap_x2*kd; 
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33 

34 

35 

36 

37 


f (6, 3) =-g/r_e; 

f  (6,  4)  =-x  (7)  *air  p*x  (  6)  *  (v*k;d  m*mv  x4+v  x4*k;d) +2*om  v(2); 
f  (6,  5)  =-x  (7)  *air  p*x  (  6)  *  (v*k;d  m*mv  x5*+v  x5*k;d) -2*om  v  ( 1 )  ; 
f  (6,  6)  =-x  (7)  *air  p*  (v*k;d  m*mv  x6*x(6)+v  x6*k;d*x  ( 6) +k;d*v)  ; 
f(6,7)=-air  p*k;d*v*x  ( 6)  ; 


S7dx 

1  function  dx=S7dx  (x,  air  p,k;d,v,r  e,om  v,g) 

2  dx=zeros (7,1); 

3  dx ( 1 ) =x ( 4 )  ; 

4  dx ( 2 ) =x ( 5 ) ; 

5  dx ( 3 ) =x ( 6 ) ; 

6  dx  ( 4  )  =-x  ( 7  )  *air  p*k;d*v*x  (4) -g*x  (1) /r  e-om  v(2)*x(6); 

7  dx  ( 5 )  =-x  ( 7 )  *air  p*k;d*v*x  (5) -g*  (1- 

sqrt (x(l) *x(l)+x(3) *x(3) ) / (2*r_e) ) +om_v (1) *x (6) ; 

8  dx  (6)  =-x  (7)  *air  p*k;d*v*x  (6) -g*x  (3) /r  e+om_v  (2)  *x  (4) -om  v(l)*x(5); 
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propP 

1  function  pnew=propP (dt, f , p, q) 

2  pdot=f *p+p*f '  ; 

3  pnew=p+ (pdot+q) *dt; 
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getK 

1  function  k;=getK  (p,  h,  r ) 

2  %calculate  gain  matrix  for  a  Kalman  filter 

3  %p  is  th  ecovariance  of  the  state 

4  %h  is  the  observation  matrix 

5  %r  is  the  covariance  of  the  observations 

6  v=p  *  h ' ; 

7  vi=inv (h*v+r ) ; 

8  k=v*vi; 


Published  with  MATLAB®  7.5 


predictH 

1  function  hitloc=predictH (x,  r  e,om  v,hO,x  w, y  w, udrag, udrag  bp,g,dt) 

2  %t  is  the  prediction  time 

3  %the  other  arguments  could  be  bundled  in  a  structure  and  passed 

4  %that  way  but  that  calls  for  packing  and  unpacking 

5  h=0; 

6  %air  pressure 

7  ap  c0=1.223;  ap  c2=l . 07 le-4 ; 
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8  %mach  number 

9  v_s0=340.3;  %59F 

10  V  c=2 . 26e-5; 

11  %tend=t; 

12  t=dt; 

13  pos= [x] ; 

14  hitloc= [0;  0] ; 

15  while  (x (2) >0) 

16  air_p=ap_cO*exp (-ap_c2*x (2) ) ;  %current  air  pressure 

17  %velocity  and  mach  number 

18  v_s=v_s0*  (l-v_c*  (x  (2)  +h0)  )  .  5; 

19  v=sqrt  (  (x  (4)  -x_w)  ''2+x  (5)  ^'2+  (x  (6)  -y_w)  ''2)  ; 

20  m=v/v  s; 

21  k;=getudrag  (m,  udrag,  udrag_bp)  ;  %udrag&udrag_bp  need  2b 

22  kd=k(l); 

23  dx=S7dx (x, air  p,kd,v,r  e,om  v,g); 

24  x=x+dx*dt; 

25  t=t+dt; 

2 6  pos= [pos  X  ] ; 

2  7  end 

28  [r, c] =size (pos) ; 

29  d=pos (1 : 3, c) -pos (1 : 3, c-1) ; 

30  pr=-pos (2 , c-1 ) /d (2 ) ; 

31  hitloc(l) =pos (l,c-l)+pr*d(l) ; 

32  hitloc(2) =pos (3, c-1) +pr*d (3) ; 


initialized 
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Appendix  B.  Additional  and  Altered  Files  to  Include  Accelerometer 
Measurements 


DriverVgpsacc 

1  %function  y=driver7() 

2  %do  the  first  intialization  phase 

3  initx 

4  dt=.01; 

5  %these  are  from  the  6dof  trajectory 

6  %obviously  in  real  time  they  would  not  be  known  in  advance 

7  %these  are  used  as  the  observation  feed 

8  load  obsdat 

9  lat=45/180*pi;  %adefault  value 

10  r_e=6378000;  h0=0; 

11  X  w=0;%0  indicates  no  wind 

12  y  w=0; 

13  %air  pressure 

14  ap  c0=1.223;  ap  c2=1.071e-4; 

15  %mach  number 

16  v_s0=340.3;  %59F 

17  V  c=2 . 26e-5; 

18  %gravity 

19  g0=9. 80665; 

20  gl=.0026; 

21  g=g0* (l-gl*cos (2*lat) ) ; 

22  %corriolis 

23  omega=7 . 2 92 le-5 ; 

24  om_v= [omega*cos (lat) ; omega*sin (lat) ; 0] ; 

25  f=zeros (7) ; 

2 6  state= [x] ; 

2  7  pre=  [  ] ; 

28  ptrace=trace  (p) ; 

29  %this  is  the  counter  for  between  observation  updates 

30  %this  helps  minimize  the  effects  of  nonlinearity 

31  %many  times  these  substeps  can  be  ignored  and  the  propagation  can 

32  %be  calculated  in  one  step  between  observations 

33  obs=0; 

34  ac_obs=0; 

35  %the  next  variable  just  counts  the  observations 

36  %the  observations  start  with  count+1 

37  %note  that  this  variable  can  be  used  to  start  the  observation  stream 

38  %at  any  desired  point  of  the  trajectory 

39  count=0; 

40  acount=0; 

41  %main  loop 

42  while  x (2 ) >0 

43  %air  preasure  etc  is  triggered  by  altitude 

44  %anyhow  update  variables  that  change  as  the  state  changes 

45  air_p=ap_c0*exp (-ap_c2*x (2) ) ;  %current  air  pressure 

46  %velocity  and  mach  number 

47  V  s=v  s0*(l-v  c*  (x  (2) +h0)  ) .  5; 

48  v=sqrt ( (x ( 4 ) -X  w) ^2+x (5) ^2+ (x (6) -y  w)^2); 
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m=v/v  s;  %mach  number 

k;=getudrag  (m,  udrag,  udrag  bp);  %udrag&udrag_bp  need  2b  initialized 
kd=k(l) ;kd_m=k(2)  ; 

f=S7calcF (x,  air  p, kd, kd  m,v,m,r  e,om  v, ap  c2,g,f); 

dx=S7dx (x,  air  p,kd,v,r  e,om  v,g); 

x=x+dx*dt; 

p=propP (dt, f , p, q) ; 

obs=obs+l ; 

ac_obs=ac_obs+l ; 

%this  inside  loop  is  triggered  by  a  GPS  observation  being  available 
if  obs==10 

count=count+l ; 

%add  some  error  to  the  observation 
z_er=diag (randn (3,1)) *r_sd* . 1; 
z=obsdat ( : , count) +z_er; 

%reset  counter 
obs=0 ; 

%if  r  is  variable  the  routine  getR  should  be  designed  to  model 

%the  observation  error  and  included  here 

K=getK (p, h, r ) ; 

x=x+K* (z-h*x) ; 

p= ( I-K*h) *p; 

%predict  routine  this  will  change  somewhat  based  on  specific 
%application 

%pos=predictX (x, r_e, om_v, hO, x_w, y_w, udrag, udrag_bp, g, dt, 5) ; 
pos=predictH (x, r_e, om_v, hO, x_w, y_w, udrag, udrag_bp, g, dt) ; 
pre=[pre  pos] ; 

end 

%note  that  GPS  observations  are  processed  first 
if  ac_obs==5 

acount=acount+l ; 

%first  find  the  observation 
a_axis=accel (x, air_p, kd, v, r_e, om_v, g) ; 

%add  some  error  to  the  observation 
r_a=abs (10*a_axis) ; 
za=a  axis+randn ( 1 ) *r  a; 

%reset  counter 
ac_obs=0 ; 

%  the  H  matrix  needs  to  be  calculated  for  each  observation  a  call 
%  to  the  routine  hmatrix  does  this 
h  a=hmatrix (x, air  p,kd,v,r  e,om  v,g); 

%if  r  is  variable  the  routine  getR  should  be  designed  to  model 

%the  observation  error  and  included  here 

K=getK (p, h_a, r_a) ; 

x=x+K*(za-h  a*x) ; 

p= ( I-K*h_a) *p; 

%predict  routine  this  will  change  somewhat  based  on  specific 
%application 

%pos=predictX (x, r_e, om_v, hO, x_w, y_w, udrag, udrag_bp, g, dt, 5) ; 

%  pos=predictH (x, r_e, om_v, hO, x_w, y_w, udrag, udrag_bp, g, dt) ; 

%  pre=[pre  pos]; 
end 

state=[state  x] ; 
ptrace= [ptrace  trace (p) ] ; 

end 

y=state ( [ 1  3  2 ] , : ) ; 
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Accel 

1  function  a  axis=accel  (x,  air  p,k;d,v,r  e,om  v,g) 

2  %calculates  the  force  measured  by  an  axial  acclerometer 

3  %uses  the  inner  product  of  the  force  and  the  normalized  velocity 

4  %assumes  the  velocity  is  aligned  with  the  spin  axis 

5  %7  dimensional  dynamics 

6  dx=zeros (3,1); 

7  dx ( 1 ) =-x ( 7 ) *air  p*kd*v*x ( 4 ) -g*x ( 1 ) /r  e-om  v(2)*x(6); 

8  dx (2) =-x (7) *air  p*kd*v*x (5) -g* (1- 

sqrt (x(l) *x(l)+x(3) *x(3) ) / (2*r_e) ) +om_v (1) *x (6) ; 

9  dx ( 3 ) =-x ( 7 ) *air  p*kd*v*x ( 6) -g*x ( 3 ) /r  e+om  v(2)*x(4)-om  v(l)*x(5); 

10  a  axis=dx ' *x (4 : 6) /v; 
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Hmatrix 

1  function  hmatrix=hmatrix (x, air  p,kd,v,r  e,om  v,g) 

2  xl3=(x(l) ^2+x(3) ^2) ^.5; 

3  dx=zeros (7,1); 

4  dx ( 1 ) =-g*x ( 4 ) /r  e/v+g*x (5) / (2*r  e) /xl3*x ( 1 ) /v; 

5  dx (3) =-g*x (6) /r_e/v+g*x (5) *x (3) /xl3/r_e/v; 

6  dx ( 4 ) =-x ( 7 ) *air  p*kd*x ( 4 ) *2-g*x ( 1 ) /r  e; 

7  dx (5)=-x(7)*2*air  p*kd*x(5)-g/v*(l-xl3/(2*r  e)) +om  v (1) *x (6) ; 

8  dx (6) =-x (7) *2*air  p*kd*v*x ( 6) -g*x ( 3 ) /r  e/v; 

9  dx(7)=-air  p*kd*v*v; 

10  hmatrix=dx'; 
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Intentionally  Lelt  Blank. 
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Appendix  C.  Additional  Routines  of  Potential  Interest 


InitsS  with  drag,  drift,  and  spin 

1  f=zeros (8,8); 

2  f(l,4)=l; 

3  f(2,5)=l; 

4  f(3,6)=l; 

5 

6  drl=[ .27754e-4  0000]; 

7  dr2=[ .29407446e-3  - . 16856609e-2  .39541129e-2  - . 40706187e-2  . 154 97302e-2 ] ; 

8  dr3=[-. 56492074  .24140455el  -.38636028el  .27445913el  -.73004070]; 

9  dr4=[.16449122el  -.6472231el  .95427249el  -.62486232el  .  15332897el] ; 

10  dr5=[-.38991679e-2  .12251269e-l  - . 14008227e-l  .70697832e-2  - . 13323438e-2 ] ; 

11  dr6=[.17693159e-3  - . 14042065e-3  .74643008e-4  - . 21621397e-4  . 2 67 8 842 6e-5 ] ; 

12 

13  drftl=[ . 94178026e-2  - . 26634522e-2  .61690538e-2  - . 32734184e-2  -.33347962e- 
2]  ; 

14  drft2=[ .10546990e2  -.48064499e2  .82211585e2  -.62499311e2  . 17816323e2] ; 

15  drft3=[ .26615371e2  -.10493487e3  .15495878e3  -.10156664e3  . 24 9367 1 le2 ] ; 

16  drft4=[-. 48373662  .14761935el  -.16536581el  .82453192  -.15394547]; 

17  drft5=[ .156258460-1  - . 16050604e-l  .17887456e-l  - . 69302356e-2  .96186882e- 
3]  ; 

18 

19  dspinl=[ .7e-2  - . 26504608e-2  - . 90103102e-3  .2528689e-2  - . 11479416e-2] ; 

20  dspin2=[.6724987e-2  - . 24994776e-2  .71838136e-3  - . 12021482e-3  .80635505e- 
5]  ; 

21 

22  udrag_bp=[.6  .9  .99  1.06  1.5  5]  ; 

23  udrag= [drl;dr2;dr3;dr4;dr5;dr6] ; 

24 

25  udrft_bp=[.84  .965  1.07  1.5  4]; 

2  6  udrf t= [drf tl ; drf t2 ; drf t3 ; drf t4  ;  drf t5  ]  ; 

27 

28  uspin= [dspinl ; dspin2 ] ; 

29  uspin_bp= [ . 9  2.5]; 


Published  with  MATLAB®  7.5 


Getudrft  gets  the  drift 

1  function  y=getudrft (m, drft, bp) 

2 

3 

4 

5 

6  row=l; 

7  if  m<bp ( 3 ) 

8  if  m>bp (2 ) 

9  row=3; 
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10 

11 

12 

13 

14 

15 

16 

17 

18 

19 

20 
21 
22 

23 

24 

25 

26 

27 

28 

29 
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else 

if  m>bp ( 1 ) 
row=2 ; 

else 

row=l ; 

end 

end 

else 

if  m<bp ( 4  ) 
row=4 ; 

else 

if  m<bp ( 5 ) 
row=5 ; 

else 

row=0 ; 

end 

end 

end 

x=[l;  m;  m*m;  m*m*m;  m*m*m*m] ; 
y=drft (row, : ) *x; 
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Getuspin 

1  function  y=getuspin (m, spin, bp) 

2  if  m<bp ( 1 ) 

3  row=l; 

4  else 

5  row=2 ; 

6  end 

7  x= [ 1 ;  m;  m*m;  m*m*m;  m*m*m*m] ; 

8  y=spin ( row, : ) *x; 
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predictX  predicted  location  in  t  time  units 

1  function  pos=predictX (x, r_e, om_v, hO, x_w, y_w, udrag, udrag_bp, g, dt,  t) 

2 

3 

4 

5  h=0; 

6 

7  ap  c0=1.223;  ap  c2=l . 07 le-4 ; 

8  ~ 

9  v_s0=340.3; 

10  V  c=2 . 26e-5; 

11  tend=t; 

12  t=dt; 

13  pos= [0;x] ; 

14  while  (t<tend) 

15  air_p=ap_c0*exp (-ap_c2*x (2) ) ; 
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16 

17  V  s=v  sO*(l-v  c*  (x  (2) +hO)  ) .  5; 

18  v=sqrt  (  (x  (4)  -x_w)  ''2+x  (5)  ''2+  (x  (6)  -y_w)  ''2)  ; 

19  m=v/v  s; 

20  k=getudrag (m, udrag, udrag_bp) ; 

21  kd=k(l) ;kd_m=k(2) ; 

22  dx=S7dx (x, air  p,kd,v,r  e,om  v,g); 

23  x=x+dx*dt; 

24  t=t+dt; 

25  pos=[pos  [t;x]  ] ; 

2  6  end 

27  pos=x; 
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end 
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